#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include "Vector3.h"

/* compile with -lpthread and -llo */

#include <pthread.h>
#include "lo/lo.h"

#define UPDATE_PATH "/pfs/listener/0001/updateAxes"
#define INFINITY_COMMAND "/infinity/target"
#define PLC_NUMBER 7
#define LISTEN_PORT "25669"
#define LOCAL_PORT "25670"
#define LOOPS 2000
#define NUM_AXES 8

void * server_thread( void * arg );
void * position_reporter( void * arg);
void * target_reporter( void * arg);

int emergencystop = 0;
int count = 0;
int updatecount = 0;
int targetcount = 0;
int firstupdates = 0;
int done = 0;

int oldspeed = 0;
int speed = 0;
int aspeed = 0;
int bspeed = 0;
int cspeed = 0;
int dspeed = 0;

lo_address t;

pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;

int axis_position[NUM_AXES] = { 1 };
int axis_status[NUM_AXES] = { 0 };
int axis_error[NUM_AXES] = { 0 };
int axis_number[NUM_AXES] = { 0 };
int axis_load[NUM_AXES] = { 0 };

int axis_target[3] = { 0 };

lo_address local_broadcast;

Vector3 wincha;
Vector3 winchb;
Vector3 winchc;
Vector3 winchd;

int amin = 25943;
int bmin = 25625;
int cmin = 25758;
int dmin = 25310;

int amax = 36043;
int bmax = 36772;
int cmax = 36922;
int dmax = 37698;

int aoffset = 3968;
int boffset = 3245;
int coffset = 3096;
int doffset = 2318;

int sn = 0;
int check = 1;

Vector3 position;
Vector3 starting;
Vector3 target;

Vector3 A,B,C,D;

void error(int num, const char *m, const char *path);

int update_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int target_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int emergency_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int state_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int playback_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int main() {
	pthread_t tid[3];
	int rc;

  t = lo_address_new("192.168.2.57", "25666");

	local_broadcast = lo_address_new("192.168.2.89", LOCAL_PORT);

	wincha.x = 0;
	wincha.y = 0;
	wincha.z = 0;

	winchb.x = 9753;
	winchb.y = 0;
	winchb.z = 548;

	winchc.x = 9753;
	winchc.y = 0;
	winchc.z = 11643;

	winchd.x = 0;
	winchd.y = 0;
	winchd.z = 12192;

	position.x = 5000;
	position.y = 5000;
	position.z = 6000;

	target.x = 5000;
	target.y = 5000;
	target.z = 6000;

	starting.x = 5000;
	starting.y = 5000;
	starting.z = 6000;

	printf("Creating threads\n");
	rc = pthread_create( &tid[0], NULL, server_thread, NULL );
	if ( rc != 0 ) {
		perror( "Could not create thread" );
	}
	rc = pthread_create( &tid[1], NULL, position_reporter, NULL );
	if ( rc != 0 ) {
		perror( "Could not create thread" );
	}
	rc = pthread_create( &tid[2], NULL, target_reporter, NULL );
	if ( rc != 0 ) {
		perror( "Could not create thread" );
	}
	printf("Threads created\n");

	pthread_join( tid[0], NULL );
	printf( "Terminating.\n" );
	pthread_mutex_destroy(&mutex);

	return 0;
}

void loadstate(int state){
  //sn++;
  if(sn > 65535)
    sn = 0;
  lo_message message = lo_message_new();
  lo_message_add(message, "ii", sn, state);
  lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
  lo_bundle_add_message(bundle, "/pfs/ur/0006/loadState", message);
  lo_send_bundle(t, bundle);
  lo_bundle_free(bundle);
  lo_message_free(message);
}

void startplayback(char* path){
  //sn++;
  if(sn > 65535)
    sn = 0;
  lo_message message = lo_message_new();
  lo_message_add(message, "i", sn);
  lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
  lo_bundle_add_message(bundle, path, message);
  lo_send_bundle(t, bundle);
  lo_bundle_free(bundle);
  lo_message_free(message);
}

void sendtoposition(char* path, int axis, int position){
  //sn++;
  if(sn > 65535)
    sn = 0;
  lo_message message = lo_message_new();
  lo_message_add(message, "iii", sn, axis, position);
  lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
  lo_bundle_add_message(bundle, path, message);
  lo_send_bundle(t, bundle);
  lo_bundle_free(bundle);
  lo_message_free(message);
}

void setspeed(char* path, int speed){
    //sn++;
    if(sn > 65535)
      sn = 0;
    lo_message message = lo_message_new();
    lo_message_add(message, "ii", sn, speed);
    lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
    lo_bundle_add_message(bundle, path, message);
    lo_send_bundle(t, bundle);
    lo_bundle_free(bundle);
    lo_message_free(message);
}

void error(int num, const char *msg, const char *path)
{
	printf("liblo server error %d in path %s: %s\n", num, path, msg);
	fflush(stdout);
}

int update_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
	int PLC = argv[0]->i;
	if(PLC != PLC_NUMBER)
		return 1;
  pthread_mutex_lock(&mutex);
  int i;
  for(i = 0; i < NUM_AXES; i++){
    axis_number[i] = argv[(i*5)+2]->i;
    axis_position[i] = argv[(i*5)+3]->i;
    axis_load[i] = argv[(i*5)+4]->i;
    axis_status[i] = argv[(i*5)+5]->i;
    axis_error[i] = argv[(i*5)+6]->i;
  }

  updatecount++;
  pthread_mutex_unlock(&mutex);
	fflush(stdout);

	return 1;
}

Vector3 clamptobox(Vector3 temp){
	double holder = 0;
	int clampcheck = 0;
	Vector3 temp_;
	if(position.z > 10000 && temp.z > 10000){
		temp.z = 10000;
		position.z = 10000;
	}
	if(position.z < 2000 && temp.z < 2000){
		temp.z = 2000;
		position.z = 2000;
	}
	if(position.y > 6077 && temp.y > 6077){
		temp.y = 6077;
		position.y = 6077;
	}
	if(position.y < 2850 && temp.y < 2850){
		temp.y = 2850;
		position.y = 2858;
	}
	if(position.x > 8300 && temp.x > 8300){
		temp.x = 8300;
		position.x = 8300;
	}
	if(position.x < 2500 && temp.x < 2500){
		temp.x = 2500;
		position.x = 2500;
	}
	Vector3 solution = temp;
	if(temp.z > 10000){
		double reduction = (10000 - position.z)/(temp.z - position.z);
		temp_.x = ((temp.x - position.x) * reduction) + position.x;
		temp_.y = ((temp.y - position.y) * reduction) + position.y;
		temp_.z = ((temp.z - position.z) * reduction) + position.z;
		if(clampcheck == 0 || vecLength(vecSub(temp_, position)) < holder){
			clampcheck = 1;
			holder = vecLength(vecSub(temp_, position));
			solution = temp_;
		}
	}
	if(temp.z < 2000){
		double reduction = (position.z - 2000)/(position.z - temp.z);
		temp_.x = ((temp.x - position.x) * reduction) + position.x;
		temp_.y = ((temp.y - position.y) * reduction) + position.y;
		temp_.z = ((temp.z - position.z) * reduction) + position.z;
		if(clampcheck == 0 || vecLength(vecSub(temp_, position)) < holder){
			clampcheck = 2;
			holder = vecLength(vecSub(temp_, position));
			solution = temp_;
		}
	}
	if(temp.y > 6077){
		double reduction = (6077 - position.y)/(temp.y - position.y);
		temp_.x = ((temp.x - position.x) * reduction) + position.x;
		temp_.y = ((temp.y - position.y) * reduction) + position.y;
		temp_.z = ((temp.z - position.z) * reduction) + position.z;
		if(clampcheck == 0 || vecLength(vecSub(temp_, position)) < holder){
			clampcheck = 3;
			holder = vecLength(vecSub(temp_, position));
			solution = temp_;
			//printf("%f reduction %d value\n", reduction, (int)solution.y);
		}
	}
	if(temp.y < 2850){
		double reduction = (position.y - 2850)/(position.y - temp.y);
		temp_.x = ((temp.x - position.x) * reduction) + position.x;
		temp_.y = ((temp.y - position.y) * reduction) + position.y;
		temp_.z = ((temp.z - position.z) * reduction) + position.z;
		if(clampcheck == 0 || vecLength(vecSub(temp_, position)) < holder){
			clampcheck = 4;
			holder = vecLength(vecSub(temp_, position));
			solution = temp_;
		}
	}
	if(temp.x < 2500){
		double reduction = (position.x - 2500)/(position.x - temp.x);
		temp_.x = ((temp.x - position.x) * reduction) + position.x;
		temp_.y = ((temp.y - position.y) * reduction) + position.y;
		temp_.z = ((temp.z - position.z) * reduction) + position.z;
		if(clampcheck == 0 || vecLength(vecSub(temp_, position)) < holder){
			clampcheck = 5;
			holder = vecLength(vecSub(temp_, position));
			solution = temp_;
		}
	}
	if(temp.x > 8300){
		double reduction = (8300 - position.x)/(temp.x - position.x);
		temp_.x = ((temp.x - position.x) * reduction) + position.x;
		temp_.y = ((temp.y - position.y) * reduction) + position.y;
		temp_.z = ((temp.z - position.z) * reduction) + position.z;
		if(clampcheck == 0 || vecLength(vecSub(temp_, position)) < holder){
			clampcheck = 6;
			holder = vecLength(vecSub(temp_, position));
			solution = temp_;
		}
	}
	//printf("%d CHECK\n", clampcheck);
        if(clampcheck != 0){
          printf("Clamping %d\n", clampcheck);
        }
	return solution;
}

int target_handler(const char *path, const char *types, lo_arg **argv, int argc,
		void *data, void *user_data){
  pthread_mutex_lock(&mutex);
  Vector3 temp;
  targetcount++;
  temp.x = argv[0]->f;
  temp.y = argv[1]->f;
  temp.z = argv[2]->f;

  if(vecLength(vecSub(temp, position)) > 20){
    temp = clamptobox(temp);
  }
  //printf("%f actual\n", temp.y);
  if(firstupdates < 40){
    target = temp;
    firstupdates ++;
  }
  /*else if((int)vecLength(vecSub(position, temp)) < 250 && check == 0){
    temp = vecAdd(vecMult(vecDiv(vecSub(target, position), vecLength(vecSub(target, position))), 250), position);
    target = temp;
    check = 1;
    //printf("HERE!!!\n", (int)vecLength(vecSub(position,target)));
  }
  else if((int)vecLength(vecSub(position,temp)) > 250 && updatecount > 0){
    target = temp;
    //printf("otherhere\n");
    check = 0;
  }
  else{
    pthread_mutex_unlock(&mutex);
    return 1;
  }*/

  target = temp;

  speed = argv[3]->i;

  int a = (int)vecLength(vecSub(wincha, target));
  int b = (int)vecLength(vecSub(winchb, target));
  int c = (int)vecLength(vecSub(winchc, target));
  int d = (int)vecLength(vecSub(winchd, target));
  a = a - aoffset;
  b = b - boffset;
  c = c - coffset;
  d = d - doffset;
  a = amax - a;
  b = bmax - b;
  c = cmax - c;
  d = dmax - d;
  axis_target[0] = a;
  axis_target[1] = b;
  axis_target[2] = c;
  axis_target[3] = d;
  pthread_mutex_unlock(&mutex);
	return 1;
}

void * position_reporter(void * arg){
	int i;
	while(1){
		pthread_mutex_lock(&mutex);
		position.x = starting.x;
		position.y = starting.y;
		position.z = starting.z;
		for( i = 0; i < LOOPS; i++){
			A = vecSub(position, wincha);
			B = vecSub(position, winchb);
			C = vecSub(position, winchc);
			D = vecSub(position, winchd);
			A = vecAdd(vecMult(A,((double)(amax-axis_position[1]+aoffset))/vecLength(A)), wincha);
			B = vecAdd(vecMult(B,((double)(bmax-axis_position[2]+boffset))/vecLength(B)), winchb);
			C = vecAdd(vecMult(C,((double)(cmax-axis_position[5]+coffset))/vecLength(C)), winchc);
			D = vecAdd(vecMult(D,((double)(dmax-axis_position[6]+doffset))/vecLength(D)), winchd);
			position = vecAdd(A,B);
			position = vecAdd(position,C);
			position = vecAdd(position,D);
			position = vecDiv(position,4);
		}
		count++;
		pthread_mutex_unlock(&mutex);
		if (lo_send(local_broadcast, "/infinity/update", "fff", position.x, position.y, position.z) == -1) {
			printf("OSC error %d: %s\n", lo_address_errno(local_broadcast), lo_address_errstr(local_broadcast));
		}
		usleep(50000);
	}
}

void * target_reporter(void * arg){
  while(1){
    pthread_mutex_lock(&mutex);
    if(axis_target[0] < amax && axis_target[0] > amin && axis_target[1] < bmax && axis_target[1] > bmin && axis_target[2] < cmax && axis_target[2] > cmin && axis_target[3] < dmax && axis_target[3] > dmin){
      double high = 0;
      int a = abs(axis_target[0]-axis_position[1]);
      int b = abs(axis_target[1]-axis_position[2]);
      int c = abs(axis_target[2]-axis_position[5]);
      int d = abs(axis_target[3]-axis_position[6]);
      if(a >= high)
        high = a;
      if(b > high)
        high = b;
      if(c > high)
        high = c;
      if(d > high)
        high = d;
      if(high > 0){
        aspeed = a/high*100;
        bspeed = b/high*100;
        cspeed = c/high*100;
        dspeed = d/high*100;
      }
      if((abs(axis_target[0]-axis_position[1]) + abs(axis_target[1]-axis_position[2]) + abs(axis_target[2]-axis_position[5]) + abs(axis_target[3]-axis_position[6])) > 20){//vecLength(vecSub(target,position)) > 25){
        int speed_ = speed;
        /*if(vecLength(vecSub(target,position)) < 750){
          if(vecLength(vecSub(target,position)) / 750.0 > 10){
            speed_ = speed_ * vecLength(vecSub(target, position)) / 750;
          }
          else{
            speed_ = 10;
          }
        }
        else if(speed_ > oldspeed){
			if(oldspeed < 10)
				oldspeed = 10;
			int temp = (double) oldspeed * 1.3;
			if(speed_ > temp){
				speed_ = temp;
			}
		}*/
	//if(oldspeed != speed_){
        	setspeed("/pfs/ur/0006/pb/1/setSpeed", (int)ceil(speed_ * ((double)aspeed/100.0)));
        	setspeed("/pfs/ur/0006/pb/2/setSpeed", (int)ceil(speed_ * ((double)bspeed/100.0)));
        	setspeed("/pfs/ur/0006/pb/3/setSpeed", (int)ceil(speed_ * ((double)cspeed/100.0)));
        	setspeed("/pfs/ur/0006/pb/4/setSpeed", (int)ceil(speed_ * ((double)dspeed/100.0)));
	//}
	oldspeed = speed_;
        sendtoposition("/pfs/ur/0006/pb/1/axis/0002/setposition", 2, axis_target[0]);
        sendtoposition("/pfs/ur/0006/pb/2/axis/0003/setposition", 3, axis_target[1]);
        sendtoposition("/pfs/ur/0006/pb/3/axis/0006/setposition", 6, axis_target[2]);
        sendtoposition("/pfs/ur/0006/pb/4/axis/0007/setposition", 7, axis_target[3]);
      }
      else{
        setspeed("/pfs/ur/0006/pb/1/setSpeed", 0);
        setspeed("/pfs/ur/0006/pb/2/setSpeed", 0);
        setspeed("/pfs/ur/0006/pb/3/setSpeed", 0);
        setspeed("/pfs/ur/0006/pb/4/setSpeed", 0);
      }
    }
    else{
      printf("OUT OF RANGE %d %d %d %d\n", axis_target[0], axis_target[1], axis_target[2], axis_target[3]);
    }
    pthread_mutex_unlock(&mutex);
    usleep(200000);
  }
}

void * server_thread( void * arg ) {
	printf("In thread\n");
	/* start a new server on port LISTEN_PORT */
	lo_server_thread st = lo_server_thread_new(LISTEN_PORT, error);

	/* add function to catch UPDATE_PATH OSC commands */
	lo_server_thread_add_method(st, UPDATE_PATH, NULL, update_handler, NULL);

  lo_server_thread_add_method(st, INFINITY_COMMAND, "fffi", target_handler, NULL);

  lo_server_thread_add_method(st, "/stop", NULL, emergency_handler, NULL);

  lo_server_thread_add_method(st, "/state", "i", state_handler, NULL);

  lo_server_thread_add_method(st, "/playback", "i", playback_handler, NULL);

	lo_server_thread_start(st);

	while (!done) {
		//Polling Loop
		usleep(1000000);
		pthread_mutex_lock(&mutex);
    int i;
    if(updatecount == 0){
      printf("NO UPDATES!\n");
    }
    else{
      if(target.x == 0 && target.y == 0 && target.z == 0){
        target.x = position.x;
        target.y = position.y;
        target.z = position.z;

        int a = (int)vecLength(vecSub(wincha, target));
        int b = (int)vecLength(vecSub(winchb, target));
        int c = (int)vecLength(vecSub(winchc, target));
        int d = (int)vecLength(vecSub(winchd, target));
        a = a - aoffset;
        b = b - boffset;
        c = c - coffset;
        d = d - doffset;
        a = amax - a;
        b = bmax - b;
	c = cmax - c;
	d = dmax - d;

        axis_target[0] = a;
        axis_target[1] = b;
        axis_target[2] = c;
        axis_target[3] = d;
      }
      for(i = 0; i < NUM_AXES; i++){
        printf("Axis: %d Pos: %d Load: %d Status: %d Error: %d\n", axis_number[i], axis_position[i], axis_load[i], axis_status[i], axis_error[i]);
      }
      printf("Position X: %f Y: %f Z: %f count: %d\n", position.x, position.y, position.z, count);
      printf("Target X: %f Y: %f Z: %f count: %d\n", target.x, target.y, target.z, targetcount);
      printf("A length: %d B length: %d C length: %d D length: %d\n", (amax-axis_position[1]+aoffset), (bmax-axis_position[2]+boffset), (cmax-axis_position[5]+coffset), (dmax-axis_position[6]+doffset));
      printf("PREDICTED: A length: %d B length %d C length %d D length %d\n",
        (int)vecLength(vecSub(wincha, position)),
        (int)vecLength(vecSub(winchb, position)),
        (int)vecLength(vecSub(winchc, position)),
        (int)vecLength(vecSub(winchd, position)));
      printf("Target: A length: %d B length %d C length %d D length %d\n", (amax-axis_target[0]+aoffset), (bmax-axis_target[1]+boffset), (cmax-axis_target[2]+coffset), (dmax-axis_target[3]+doffset));
      printf("Aspeed: %d Bspeed: %d Cspeed: %d Dspeed: %d\n",(int)ceil(speed * ((double)aspeed/100.0)),(int)ceil(speed * ((double)bspeed/100.0)), (int)ceil(speed * ((double)cspeed/100.0)), (int)ceil(speed * ((double)dspeed/100.0)));
      updatecount = 0;
      targetcount = 0;
      count = 0;
    }
		pthread_mutex_unlock(&mutex);
	}

	lo_server_thread_free(st);

	return 0;
}

int emergency_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
  printf("GOT!\n");
  pthread_mutex_lock(&mutex);
  while(1){
    printf("EMERGENCY STOP!\n");
    setspeed("/pfs/ur/0006/pb/1/setSpeed", 0);
    setspeed("/pfs/ur/0006/pb/2/setSpeed", 0);
    setspeed("/pfs/ur/0006/pb/3/setSpeed", 0);
    setspeed("/pfs/ur/0006/pb/4/setSpeed", 0);
    sendtoposition("/pfs/ur/0006/pb/1/axis/0002/setposition", 2, axis_position[1]);
    sendtoposition("/pfs/ur/0006/pb/2/axis/0003/setposition", 3, axis_position[2]);
    sendtoposition("/pfs/ur/0006/pb/3/axis/0006/setposition", 6, axis_position[5]);
    sendtoposition("/pfs/ur/0006/pb/4/axis/0007/setposition", 7, axis_position[6]);
    usleep(50000);
  }
}

int state_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
  pthread_mutex_lock(&mutex);
  loadstate(argv[0]->i);
  pthread_mutex_unlock(&mutex);
}

int playback_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
  pthread_mutex_lock(&mutex);
  if(argv[0]->i<1 || argv[0]->i > 6)
    return 0;
  char str[30];
  sprintf(str, "/pfs/ur/0006/pb/%d/start", argv[0]->i);
  startplayback(str);
  pthread_mutex_unlock(&mutex);
}
